(in-package #:org.shirakumo.fraf.trial)

(defmethod 3ds:call-with-all (function (scene physics-scene))
  (3ds:call-with-all function (physics-system scene)))

(defmethod 3ds:call-with-candidates (function (scene physics-scene) region)
  (3ds:call-with-candidates function (physics-system scene) region))

(defmethod 3ds:call-with-contained (function (scene physics-scene) region)
  (3ds:call-with-contained function (physics-system scene) region))

(defmethod 3ds:call-with-intersecting (function (scene physics-scene) origin direction)
  (3ds:call-with-intersecting function (physics-system scene) origin direction))

(defmethod 3ds:call-with-overlapping (function (scene physics-scene) region)
  (3ds:call-with-overlapping function (physics-system scene) region))

(defmethod 3ds:call-with-pairs (function (scene physics-scene))
  (3ds:call-with-pairs function (physics-system scene)))

(defmethod 3ds:serialize ((scene physics-scene) file object->id)
  (3ds:serialize (physics-system scene) file object->id))

(defmethod 3ds:deserialize ((scene physics-scene) file id->object)
  (3ds:deserialize (physics-system scene) file id->object))

(defmethod 3ds:reoptimize ((scene physics-scene) &rest args &key &allow-other-keys)
  (apply #'3ds:reoptimize (physics-system scene) args))

(defclass accelerated-rigidbody-system (rigidbody-system)
  ((dynamic-acceleration-structure :initform (org.shirakumo.fraf.trial.space.kd-tree:make-kd-tree) :accessor dynamic-acceleration-structure)
   (static-acceleration-structure :initform (org.shirakumo.fraf.trial.space.kd-tree:make-kd-tree) :accessor static-acceleration-structure)
   (global-primitives :initform (make-array 128 :fill-pointer 0) :accessor global-primitives)
   (pending-inserts :initform (make-array 128 :fill-pointer 0) :accessor pending-inserts)))

(defmethod 3ds:call-with-all (function (system accelerated-rigidbody-system))
  (3ds:call-with-all function (dynamic-acceleration-structure system))
  (3ds:call-with-all function (static-acceleration-structure system)))

(defmethod 3ds:call-with-candidates (function (system accelerated-rigidbody-system) region)
  (3ds:call-with-candidates function (dynamic-acceleration-structure system) region)
  (3ds:call-with-candidates function (static-acceleration-structure system) region))

(defmethod 3ds:call-with-contained (function (system accelerated-rigidbody-system) region)
  (3ds:call-with-contained function (dynamic-acceleration-structure system) region)
  (3ds:call-with-contained function (static-acceleration-structure system) region))

(defmethod 3ds:call-with-intersecting (function (system accelerated-rigidbody-system) origin direction)
  (3ds:call-with-intersecting function (dynamic-acceleration-structure system) origin direction)
  (3ds:call-with-intersecting function (static-acceleration-structure system) origin direction))

(defmethod 3ds:call-with-overlapping (function (system accelerated-rigidbody-system) region)
  (3ds:call-with-overlapping function (dynamic-acceleration-structure system) region)
  (3ds:call-with-overlapping function (static-acceleration-structure system) region))

(defmethod 3ds:call-with-pairs (function (system accelerated-rigidbody-system))
  (3ds:call-with-pairs function (dynamic-acceleration-structure system))
  (3ds:call-with-pairs function (static-acceleration-structure system)))

(defmethod 3ds:reoptimize ((system accelerated-rigidbody-system) &rest args &key &allow-other-keys)
  (v:info :trial.physics "Reoptimizing ~a" system)
  (apply #'3ds:reoptimize (static-acceleration-structure system) args)
  (apply #'3ds:reoptimize (dynamic-acceleration-structure system) args))

(defmethod 3ds:serialize ((system accelerated-rigidbody-system) file object->id)
  (3ds:serialize (static-acceleration-structure system) file object->id))

(defmethod 3ds:deserialize ((system accelerated-rigidbody-system) file id->object)
  (setf (static-acceleration-structure system)
        (3ds:deserialize (static-acceleration-structure system) file id->object)))

(defmethod dynamic-entity-p ((body rigid-shape))
  (< 0 (the single-float (inverse-mass body))))

(defmethod enter :before ((body rigid-shape) (system accelerated-rigidbody-system))
  (cond ((= 0 (length (physics-primitives body)))
         (unless (find body (pending-inserts system))
           (vector-push-extend body (pending-inserts system))))
        (T
         (start-frame body)
         (loop with structure = (if (dynamic-entity-p body)
                                    (dynamic-acceleration-structure system)
                                    (static-acceleration-structure system))
               for primitive across (physics-primitives body)
               do (if (typep primitive 'all-space)
                      (vector-push-extend primitive (global-primitives system))
                      (3ds:enter primitive structure))))))

(defmethod leave :after ((body rigid-shape) (system accelerated-rigidbody-system))
  (unless (array-utils:vector-pop-element* (pending-inserts system) body)
    (loop with structure = (if (dynamic-entity-p body)
                               (dynamic-acceleration-structure system)
                               (static-acceleration-structure system))
          for primitive across (physics-primitives body)
          do (if (typep primitive 'all-space)
                 (array-utils:vector-pop-element* (global-primitives system) primitive)
                 (3ds:leave primitive structure)))))

(defmethod integrate :after ((system accelerated-rigidbody-system) dt)
  (declare (optimize speed))
  (loop with structure = (dynamic-acceleration-structure system)
        for object across (the (and (vector T) (not simple-array)) (%objects system))
        do (when (dynamic-entity-p object)
             (loop for primitive across (the simple-vector (physics-primitives object))
                   do (unless (typep primitive 'all-space)
                        (3ds:update primitive structure))))))

(defmethod detect-hits ((system accelerated-rigidbody-system) other hits start end)
  (flet ((try (thing)
           (setf start (prune-hits hits start (detect-hits thing other hits start end)))))
    (try (static-acceleration-structure system))
    (try (dynamic-acceleration-structure system))
    (try (global-primitives system))))

(defmethod detect-hits (other (system accelerated-rigidbody-system) hits start end)
  (flet ((try (thing)
           (setf start (prune-hits hits start (detect-hits other thing hits start end)))))
    (try (static-acceleration-structure system))
    (try (dynamic-acceleration-structure system))
    (try (global-primitives system))))

(defmethod generate-hits ((system accelerated-rigidbody-system) hits start end)
  (declare (optimize speed))
  (declare (type simple-vector hits))
  (declare (type (unsigned-byte 32) start end))
  (flet ((update-start (next)
           (setf start (prune-hits hits start next))
           (when (<= end start)
             (dbg "HIT Overflow")
             (return-from generate-hits start))))
    (loop with structure = (static-acceleration-structure system)
          for object across (the (and (vector T) (not simple-array)) (%objects system))
          do (when (and (dynamic-entity-p object) (awake-p object))
               (loop for a across (the simple-vector (physics-primitives object))
                     do (3ds:do-overlapping (b structure a)
                          (when (< 0 (logand (collision-mask a) (collision-mask b)
                                             (collision-mask object) (collision-mask (primitive-entity b))))
                            (update-start (detect-hits a b hits start end)))))))
    (loop for a across (the (and (vector T) (not simple-array)) (global-primitives system))
          do (3ds:do-all (b (dynamic-acceleration-structure system))
               (when (typep b 'primitive)
                 (update-start (detect-hits a b hits start end)))))
    (3ds:do-pairs (a b (dynamic-acceleration-structure system) start)
      (let ((entity1 (primitive-entity a))
            (entity2 (primitive-entity b)))
        (when (and (< 0 (logand (collision-mask a) (collision-mask b)
                                (collision-mask entity1) (collision-mask entity2)))
                   (or (awake-p entity1) (awake-p entity2)))
          (update-start (detect-hits a b hits start end)))))))

(defmethod start-frame :before ((system accelerated-rigidbody-system))
  (let ((pending-inserts (pending-inserts system))
        (global (global-primitives system))
        (static (static-acceleration-structure system))
        (dynamic (dynamic-acceleration-structure system))
        (static-changed-p NIL))
    (loop while (< 0 (length pending-inserts))
          for body = (vector-pop pending-inserts)
          for structure = (if (dynamic-entity-p body) dynamic static)
          do (start-frame body)
             (when (eq structure static) (setf static-changed-p T))
             (loop for primitive across (physics-primitives body)
                   do (if (typep primitive 'all-space)
                          (vector-push-extend primitive global)
                          (3ds:enter primitive structure))))
    (when static-changed-p
      (3ds:reoptimize system))))

(defclass bsp-accelerated-rigidbody-system (accelerated-rigidbody-system)
  ((static-acceleration-structure :initform (org.shirakumo.fraf.trial.space.kd-tree:make-kd-tree))
   (bsp-built-p :initform NIL :accessor bsp-built-p)))

(defmethod 3ds:deserialize :after ((system bsp-accelerated-rigidbody-system) file id->object)
  (setf (bsp-built-p system) T))

(defmethod 3ds:reoptimize :after ((system bsp-accelerated-rigidbody-system) &key)
  (setf (bsp-built-p system) T))

(defmethod start-frame ((system bsp-accelerated-rigidbody-system))
  (unless (bsp-built-p system)
    (3ds:reoptimize system))
  (call-next-method))

(defmethod generate-hits ((system bsp-accelerated-rigidbody-system) hits start end)
  (declare (optimize speed))
  (declare (type simple-vector hits))
  (declare (type (unsigned-byte 32) start end))
  (flet ((update-start (next)
           (setf start (prune-hits hits start next))
           (when (<= end start)
             (dbg "HIT Overflow")
             (return-from generate-hits start))))
    (loop with structure = (static-acceleration-structure system)
          for object across (the (and (vector T) (not simple-array)) (%objects system))
          do (when (and (dynamic-entity-p object) (awake-p object))
               (loop for a across (the simple-vector (physics-primitives object))
                     do (3ds:do-overlapping (b structure a)
                          (let ((bg (3ds:group b)))
                            (when (< 0 (logand (collision-mask a) (collision-mask bg)
                                               (collision-mask object) (collision-mask (primitive-entity bg))))
                              (let ((next (detect-hits a b hits start end)))
                                (loop for i from start below next
                                      for hit = (aref hits i)
                                      do (setf (hit-b hit) bg))
                                (update-start next))))))))
    (loop for a across (the (and (vector T) (not simple-array)) (global-primitives system))
          do (3ds:do-all (b (dynamic-acceleration-structure system))
               (when (typep b 'primitive)
                 (update-start (detect-hits a b hits start end)))))
    (3ds:do-pairs (a b (dynamic-acceleration-structure system) start)
      (let ((entity1 (primitive-entity a))
            (entity2 (primitive-entity b)))
        (when (and (< 0 (logand (collision-mask a) (collision-mask b)
                                (collision-mask entity1) (collision-mask entity2)))
                   (or (awake-p entity1) (awake-p entity2)))
          (update-start (detect-hits a b hits start end)))))))
